
using System;
using System.Runtime.CompilerServices;


namespace cs5643.rigidbody
{



    /**
     * Maintains a 2D rigidbody transform.
     *
     * @author Doug James, March 2007.
     */
    public class RigidTransform
    {
        Trafo2D T;

        public RigidTransform()
        {
            T = Trafo2D.Identity();
        }

        public void set(RigidTransform transform)
        {
            T = transform.T;
        }

        /** Transformation specified by rotation angle, theta, and translation. */
        public void set(double theta, Pnt2D translation)
        {
            if (Double.IsNaN(theta)) throw new ArithmeticException("theta was NaN");
            Rot2D r = new Rot2D(theta);
            T = r.ToTrafo2D();
            T[0, 2] = translation.x;
            T[1, 2] = translation.y;
            //Console.Out.WriteLine("T="+T+", theta="+theta);
        }

        /** Inverts transform. */
        public void invert()
        {
            try
            {
                T = T.Inverse;
            }
            catch (Exception e)
            {
                Console.Out.WriteLine("e=" + e + " \n " + e.StackTrace + "\n" + T);
            }
        }

        private Pnt3D tmp = new Pnt3D(0, 0, 0);

        /** Multiply this matrix by the tuple t and place the result back into the tuple (t = this*t). */
        [MethodImpl(MethodImplOptions.Synchronized)]
        public void transform(ref Pnt2D p)
        {
            tmp.x = p.x;
            tmp.y = p.y;
            tmp.z = 1; //((p is Pnt2D) ? 1 : 0);///SLOTH
            //tmp = T * tmp;
            p.x = tmp.x * T[0, 0] + tmp.y * T[0, 1] + tmp.z * T[0, 2];
            p.y = tmp.x * T[1, 0] + tmp.y * T[1, 1] + tmp.z * T[1, 2];
            //T.transform(tmp);
            //p.x = tmp.x;
            //p.y = tmp.y;
        }
    }
}